
#include "LinearMath/btTransform.h"

#include "HumanCreature.h"

namespace jcrada {

    HumanCreature::HumanCreature( int hidden_layers, int neurons_per_layer ) :
    Creature( BODY_PARTS, CONSTRAINTS, hidden_layers, neurons_per_layer ) {
        setName("HumanCreature");
    }

    HumanCreature::~HumanCreature( ) {

    }

    void HumanCreature::set( const btTransform& at ) {
        setBodies();
        setConstraints();
        
        btTransform transform;
        double max_height = 0.0;
        max_height += getBodyPart(B_HEAD).getSizeY() + getBodyPart(B_THORAX).getSizeY() +
                getBodyPart(B_LTHIGH).getSizeY() + getBodyPart(B_LTHIGH).getSizeR()+
                getBodyPart(B_LCALF).getSizeY() +getBodyPart(B_LCALF).getSizeR() +
                getBodyPart(B_LFOOT).getSizeY() +
                getBodyPart(B_LFOOT).getSizeY();
        

        double current_height = max_height;
        //HEAD
//        current_height -= getBody(HEAD).getSizeR();
        transform.setIdentity();
        transform.setOrigin(btVector3(0.0, current_height, 0.0));
        getBodyPart(B_HEAD).set(at * transform);
        current_height -= getBodyPart(B_HEAD).getSizeR();

        //THORAX
        current_height -= getBodyPart(B_THORAX).getSizeY() / 2.0;
        transform.setIdentity();
        transform.setOrigin(btVector3(0.0, current_height, 0.0));
        getBodyPart(B_THORAX).set(at * transform);
        current_height -= getBodyPart(B_THORAX).getSizeY() / 2.0;

        double current_width = 0.0;
        //THIGHS
        current_width += getBodyPart(B_THORAX).getSizeX() / 2.0;
        current_height -= getBodyPart(B_LTHIGH).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LTHIGH).getSizeR();
        transform.setIdentity();
        transform.setOrigin(btVector3(-current_width, current_height, 0.0));
        getBodyPart(B_LTHIGH).set(at * transform);

        transform.setIdentity();
        transform.setOrigin(btVector3(current_width, current_height, 0.0));
        getBodyPart(B_RTHIGH).set(at * transform);
        current_height -= getBodyPart(B_LTHIGH).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LTHIGH).getSizeR();

        //CALVES
        current_height -= getBodyPart(B_LCALF).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LCALF).getSizeR() ;
        transform.setIdentity();
        transform.setOrigin(btVector3(-current_width, current_height, 0.0));
        getBodyPart(B_LCALF).set(at * transform);

        transform.setIdentity();
        transform.setOrigin(btVector3(current_width, current_height, 0.0));
        getBodyPart(B_RCALF).set(at * transform);
        current_height -= getBodyPart(B_LCALF).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LCALF).getSizeR() ;

        //FEET
        current_height -= getBodyPart(B_LFOOT).getSizeY() / 2.0;
        transform.setIdentity();
        transform.setOrigin(btVector3(-current_width, current_height, 0.0));
        getBodyPart(B_LFOOT).set(at * transform);

        transform.setIdentity();
        transform.setOrigin(btVector3(current_width, current_height, 0.0));
        getBodyPart(B_RFOOT).set(at * transform);
        current_height -= getBodyPart(B_LCALF).getSizeY() / 2.0;


        current_height = max_height - getBodyPart(B_HEAD).getSizeR();
        current_width = getBodyPart(B_THORAX).getSizeX() / 2.0;
        //UPPERARMS
        current_height += getBodyPart(B_LUPPERARM).getSizeX() / 2.0;
        current_height -= getBodyPart(B_LUPPERARM).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LUPPERARM).getSizeR() ;
        current_width += getBodyPart(B_LUPPERARM).getSizeX() / 2.0;

        transform.setIdentity();
        transform.setOrigin(btVector3(-current_width, current_height, 0.0));
        getBodyPart(B_LUPPERARM).set(at * transform);

        transform.setIdentity();
        transform.setOrigin(btVector3(current_width, current_height, 0.0));
        getBodyPart(B_RUPPERARM).set(at * transform);
        current_height -= getBodyPart(B_LUPPERARM).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LUPPERARM).getSizeR() ;

        //FOREARMS
        current_height -= getBodyPart(B_LFOREARM).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LFOREARM).getSizeR() ;
        transform.setIdentity();
        transform.setOrigin(btVector3(-current_width, current_height, 0.0));
        getBodyPart(B_LFOREARM).set(at * transform);

        transform.setIdentity();
        transform.setOrigin(btVector3(current_width, current_height, 0.0));
        getBodyPart(B_RFOREARM).set(at * transform);
        current_height -= getBodyPart(B_RFOREARM).getSizeY() / 2.0;
        current_height -= getBodyPart(B_LFOREARM).getSizeR() ;

        //HANDS
//        current_height -= getBody(B_LHAND).getSizeY() / 2.0;
//        transform.setIdentity();
//        transform.setOrigin(btVector3(-current_width, current_height, 0.0));
//        getBody(B_LHAND).set(at * transform);
//
//        transform.setIdentity();
//        transform.setOrigin(btVector3(current_width, current_height, 0.0));
//        getBody(B_RHAND).set(at * transform);
//        current_height -= getBody(B_LHAND).getSizeY() / 2.0;

        for (int i = 0; i < getNumberOfConstraints(); ++i) {
            getConstraint(i).set(*this);
        }
        calculateMaxTorque();
    }

    void HumanCreature::setBodies( ) {
        getBodyPart(B_HEAD).setId(B_HEAD);
        getBodyPart(B_HEAD).setName("Head");
        getBodyPart(B_HEAD).setParentId(B_THORAX);
        getBodyPart(B_HEAD).setShape(BodyPart::S_SPHERE);
        getBodyPart(B_HEAD).setSize(0.25, 0.25, 0.25);
        getBodyPart(B_HEAD).setMass(4.4);

        getBodyPart(B_THORAX).setId(B_THORAX);
        getBodyPart(B_THORAX).setName("Thorax");
        getBodyPart(B_THORAX).setParentId(Creature::B_NONE);
        getBodyPart(B_THORAX).setShape(BodyPart::S_BOX);
        getBodyPart(B_THORAX).setSize(0.305, 0.829, 0.225);
        getBodyPart(B_THORAX).setMass(33.4);

        getBodyPart(B_LUPPERARM).setId(B_LUPPERARM);
        getBodyPart(B_LUPPERARM).setName("LeftUpperArm");
        getBodyPart(B_LUPPERARM).setParentId(B_THORAX);
        getBodyPart(B_LUPPERARM).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_LUPPERARM).setSize(0.090, 0.311, 0.090);
        getBodyPart(B_LUPPERARM).setMass(2.4);

        getBodyPart(B_RUPPERARM).setId(B_RUPPERARM);
        getBodyPart(B_RUPPERARM).setName("RightUpperArm");
        getBodyPart(B_RUPPERARM).setParentId(B_THORAX);
        getBodyPart(B_RUPPERARM).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_RUPPERARM).setSize(0.090, 0.311, 0.090);
        getBodyPart(B_RUPPERARM).setMass(2.4);

        getBodyPart(B_LFOREARM).setId(B_LFOREARM);
        getBodyPart(B_LFOREARM).setName("LeftForeArm");
        getBodyPart(B_LFOREARM).setParentId(B_LUPPERARM);
        getBodyPart(B_LFOREARM).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_LFOREARM).setSize(0.084, 0.284, 0.084);
        getBodyPart(B_LFOREARM).setMass(1.6);

        getBodyPart(B_RFOREARM).setId(B_RFOREARM);
        getBodyPart(B_RFOREARM).setName("RightForeArm");
        getBodyPart(B_RFOREARM).setParentId(B_RUPPERARM);
        getBodyPart(B_RFOREARM).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_RFOREARM).setSize(0.084, 0.284, 0.084);
        getBodyPart(B_RFOREARM).setMass(1.6);

//        getBody(B_LHAND).setId(B_LHAND);
//        getBody(B_LHAND).setName("LeftHand");
//        getBody(B_LHAND).setParentId(B_LFOREARM);
//        getBody(B_LHAND).setShape(Body::S_BOX);
//        getBody(B_LHAND).setSize(0.085, 0.183, 0.027);
//        getBody(B_LHAND).setMass(0.6);
//
//        getBody(B_RHAND).setId(B_RHAND);
//        getBody(B_RHAND).setName("RightHand");
//        getBody(B_RHAND).setParentId(B_RFOREARM);
//        getBody(B_RHAND).setShape(Body::S_BOX);
//        getBody(B_RHAND).setSize(0.085, 0.183, 0.027);
//        getBody(B_RHAND).setMass(0.6);

        getBodyPart(B_LTHIGH).setId(B_LTHIGH);
        getBodyPart(B_LTHIGH).setName("LeftThigh");
        getBodyPart(B_LTHIGH).setParentId(B_THORAX);
        getBodyPart(B_LTHIGH).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_LTHIGH).setSize(0.170, 0.471, 0.170);
        getBodyPart(B_LTHIGH).setMass(11.8);

        getBodyPart(B_RTHIGH).setId(B_RTHIGH);
        getBodyPart(B_RTHIGH).setName("RightThigh");
        getBodyPart(B_RTHIGH).setParentId(B_THORAX);
        getBodyPart(B_RTHIGH).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_RTHIGH).setSize(0.170, 0.471, 0.170);
        getBodyPart(B_RTHIGH).setMass(11.8);

        getBodyPart(B_LCALF).setId(B_LCALF);
        getBodyPart(B_LCALF).setName("LeftCalf");
        getBodyPart(B_LCALF).setParentId(B_LTHIGH);
        getBodyPart(B_LCALF).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_LCALF).setSize(0.110, 0.334, 0.110);
        getBodyPart(B_LCALF).setMass(4.5);

        getBodyPart(B_RCALF).setId(B_RCALF);
        getBodyPart(B_RCALF).setName("RightCalf");
        getBodyPart(B_RCALF).setParentId(B_RTHIGH);
        getBodyPart(B_RCALF).setShape(BodyPart::S_CAPSULE_Y);
        getBodyPart(B_RCALF).setSize(0.110, 0.334, 0.110);
        getBodyPart(B_RCALF).setMass(4.5);

        getBodyPart(B_LFOOT).setId(B_LFOOT);
        getBodyPart(B_LFOOT).setName("LeftFoot");
        getBodyPart(B_LFOOT).setParentId(B_LCALF);
        getBodyPart(B_LFOOT).setShape(BodyPart::S_BOX);
        getBodyPart(B_LFOOT).setSize(0.186, 0.154, 0.257);
        getBodyPart(B_LFOOT).setMass(1.1);

        getBodyPart(B_RFOOT).setId(B_RFOOT);
        getBodyPart(B_RFOOT).setName("RightFoot");
        getBodyPart(B_RFOOT).setParentId(B_RCALF);
        getBodyPart(B_RFOOT).setShape(BodyPart::S_BOX);
        getBodyPart(B_RFOOT).setSize(0.186, 0.154, 0.257);
        getBodyPart(B_RFOOT).setMass(1.1);

    }

    void HumanCreature::setConstraints( ) {
        for (int i=  0 ; i < CONSTRAINTS ;++i){
            getConstraint(i).setType(Constraint::CT_GENERIC_6DOF);
        }
        getConstraint(C_NECK).setId(C_NECK);
        getConstraint(C_NECK).setName("Neck");
        getConstraint(C_NECK).setIdBodyA(getBodyPart(B_HEAD).getId());
        getConstraint(C_NECK).setIdBodyB(getBodyPart(B_THORAX).getId());
        getConstraint(C_NECK).setConnectionA(Constraint::SPHERE_BOTTOM);
        getConstraint(C_NECK).setConnectionB(Constraint::BOX_TOP);
        getConstraint(C_NECK).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_NECK).setAngularLowerUpperLimits(
                btVector3(-M_PI_4 / 2.0, -M_PI_4 / 2.0, -M_PI_4 / 2.0),
                btVector3(M_PI_4 / 2.0, M_PI_4 / 2.0, M_PI_4 / 2.0));

        getConstraint(C_LSHOULDER).setId(C_LSHOULDER);
        getConstraint(C_LSHOULDER).setName("LeftShoulder");
        getConstraint(C_LSHOULDER).setIdBodyA(getBodyPart(B_THORAX).getId());
        getConstraint(C_LSHOULDER).setIdBodyB(getBodyPart(B_LUPPERARM).getId());
        getConstraint(C_LSHOULDER).setConnectionA(Constraint::BOX_TOP_LEFT);
        getConstraint(C_LSHOULDER).setConnectionB(Constraint::CYLINDER_R1_RIGHT);
        getConstraint(C_LSHOULDER).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_LSHOULDER).setAngularLowerUpperLimits(
                btVector3((2.0 / 3.0) * -M_PI_2, 0, -M_PI_2),
                btVector3((2.0 / 3.0) * M_PI_2, M_PI_4, M_PI_2));

        getConstraint(C_RSHOULDER).setId(C_RSHOULDER);
        getConstraint(C_RSHOULDER).setName("RightShoulder");
        getConstraint(C_RSHOULDER).setIdBodyA(getBodyPart(B_THORAX).getId());
        getConstraint(C_RSHOULDER).setIdBodyB(getBodyPart(B_RUPPERARM).getId());
        getConstraint(C_RSHOULDER).setConnectionA(Constraint::BOX_TOP_RIGHT);
        getConstraint(C_RSHOULDER).setConnectionB(Constraint::CYLINDER_R1_LEFT);
        getConstraint(C_RSHOULDER).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_RSHOULDER).setAngularLowerUpperLimits(
                btVector3((2.0 / 3.0) * -M_PI_2, 0, -M_PI_2),
                btVector3((2.0 / 3.0) * M_PI_2, M_PI_4, M_PI_2));

        getConstraint(C_LELBOW).setId(C_LELBOW);
        getConstraint(C_LELBOW).setName("LeftElbow");
        getConstraint(C_LELBOW).setIdBodyA(getBodyPart(B_LUPPERARM).getId());
        getConstraint(C_LELBOW).setIdBodyB(getBodyPart(B_LFOREARM).getId());
        getConstraint(C_LELBOW).setConnectionA(Constraint::CYLINDER_BOTTOM);
        getConstraint(C_LELBOW).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_LELBOW).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_LELBOW).setAngularLowerUpperLimits(
                btVector3(0, 0, 0),
                btVector3(M_PI_2 + M_PI_4, 0, 0));

        getConstraint(C_RELBOW).setId(C_RELBOW);
        getConstraint(C_RELBOW).setName("RightElbow");
        getConstraint(C_RELBOW).setIdBodyA(getBodyPart(B_RUPPERARM).getId());
        getConstraint(C_RELBOW).setIdBodyB(getBodyPart(B_RFOREARM).getId());
        getConstraint(C_RELBOW).setConnectionA(Constraint::CYLINDER_BOTTOM);
        getConstraint(C_RELBOW).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_RELBOW).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_RELBOW).setAngularLowerUpperLimits(
                btVector3(0, 0, 0),
                btVector3(M_PI_2 + M_PI_4, 0, 0));

//        getConstraint(C_LWRIST).setId(C_LWRIST);
//        getConstraint(C_LWRIST).setName("LeftWrist");
//        getConstraint(C_LWRIST).setIdBodyA(getBody(B_LFOREARM).getId());
//        getConstraint(C_LWRIST).setIdBodyB(getBody(B_LHAND).getId());
//        getConstraint(C_LWRIST).setConnectionA(Constraint::CYLINDER_BOTTOM);
//        getConstraint(C_LWRIST).setConnectionB(Constraint::BOX_TOP);
//        getConstraint(C_LWRIST).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
//        getConstraint(C_LWRIST).setAngularLowerUpperLimits(
//                btVector3(0, -M_PI_4, -M_PI_4),
//                btVector3(0, M_PI_4, M_PI_4));
//
//        getConstraint(C_RWRIST).setId(C_RWRIST);
//        getConstraint(C_RWRIST).setName("RightWrist");
//        getConstraint(C_RWRIST).setIdBodyA(getBody(B_RFOREARM).getId());
//        getConstraint(C_RWRIST).setIdBodyB(getBody(B_RHAND).getId());
//        getConstraint(C_RWRIST).setConnectionA(Constraint::CYLINDER_BOTTOM);
//        getConstraint(C_RWRIST).setConnectionB(Constraint::BOX_TOP);
//        getConstraint(C_RWRIST).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
//        getConstraint(C_RWRIST).setAngularLowerUpperLimits(
//                btVector3(0, -M_PI_4, -M_PI_4),
//                btVector3(0, M_PI_4, M_PI_4));

        getConstraint(C_LHIP).setId(C_LHIP);
        getConstraint(C_LHIP).setName("LeftHuip");
        getConstraint(C_LHIP).setIdBodyA(getBodyPart(B_THORAX).getId());
        getConstraint(C_LHIP).setIdBodyB(getBodyPart(B_LTHIGH).getId());
        getConstraint(C_LHIP).setConnectionA(Constraint::BOX_BOT_LEFT);
        getConstraint(C_LHIP).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_LHIP).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_LHIP).setAngularLowerUpperLimits(
                btVector3(-M_PI_4, 0.0, -M_PI_4),
                btVector3(M_PI_2, (2.0 / 3.0) * M_PI_2, M_PI_4));

        getConstraint(C_RHIP).setId(C_RHIP);
        getConstraint(C_RHIP).setName("RightHip");
        getConstraint(C_RHIP).setIdBodyA(getBodyPart(B_THORAX).getId());
        getConstraint(C_RHIP).setIdBodyB(getBodyPart(B_RTHIGH).getId());
        getConstraint(C_RHIP).setConnectionA(Constraint::BOX_BOT_RIGHT);
        getConstraint(C_RHIP).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_RHIP).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_RHIP).setAngularLowerUpperLimits(
                btVector3(-M_PI_4, 0.0, -M_PI_4),
                btVector3(M_PI_2, (2.0 / 3.0) * M_PI_2, M_PI_4));

        getConstraint(C_LKNEE).setId(C_LKNEE);
        getConstraint(C_LKNEE).setName("LeftKnee");
        getConstraint(C_LKNEE).setIdBodyA(getBodyPart(B_LTHIGH).getId());
        getConstraint(C_LKNEE).setIdBodyB(getBodyPart(B_LCALF).getId());
        getConstraint(C_LKNEE).setConnectionA(Constraint::CYLINDER_BOTTOM);
        getConstraint(C_LKNEE).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_LKNEE).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_LKNEE).setAngularLowerUpperLimits(
                btVector3(-M_PI_2, 0.0, 0.0),
                btVector3(0.0, 0.0, 0.0));

        getConstraint(C_RKNEE).setId(C_RKNEE);
        getConstraint(C_RKNEE).setName("RightKnee");
        getConstraint(C_RKNEE).setIdBodyA(getBodyPart(B_RTHIGH).getId());
        getConstraint(C_RKNEE).setIdBodyB(getBodyPart(B_RCALF).getId());
        getConstraint(C_RKNEE).setConnectionA(Constraint::CYLINDER_BOTTOM);
        getConstraint(C_RKNEE).setConnectionB(Constraint::CYLINDER_TOP);
        getConstraint(C_RKNEE).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_RKNEE).setAngularLowerUpperLimits(
                btVector3(-M_PI_2, 0.0, 0.0),
                btVector3(0.0, 0.0, 0.0));

        getConstraint(C_LANKLE).setId(C_LANKLE);
        getConstraint(C_LANKLE).setName("LeftAnkle");
        getConstraint(C_LANKLE).setIdBodyA(getBodyPart(B_LCALF).getId());
        getConstraint(C_LANKLE).setIdBodyB(getBodyPart(B_LFOOT).getId());
        getConstraint(C_LANKLE).setConnectionA(Constraint::CYLINDER_BOTTOM);
        getConstraint(C_LANKLE).setConnectionB(Constraint::BOX_TOP);
        getConstraint(C_LANKLE).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_LANKLE).setAngularLowerUpperLimits(
                btVector3(-M_PI_4, 0.0, -M_PI_4 / 2.0),
                btVector3(0.0, 0.0, M_PI_4 / 2.0));

        getConstraint(C_RANKLE).setId(C_RANKLE);
        getConstraint(C_RANKLE).setName("RightAnkle");
        getConstraint(C_RANKLE).setIdBodyA(getBodyPart(B_RCALF).getId());
        getConstraint(C_RANKLE).setIdBodyB(getBodyPart(B_RFOOT).getId());
        getConstraint(C_RANKLE).setConnectionA(Constraint::CYLINDER_BOTTOM);
        getConstraint(C_RANKLE).setConnectionB(Constraint::BOX_TOP);
        getConstraint(C_RANKLE).setLinearLowerUpperLimits(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0));
        getConstraint(C_RANKLE).setAngularLowerUpperLimits(
                btVector3(-M_PI_4, 0.0, -M_PI_4 / 2.0),
                btVector3(0.0, 0.0, M_PI_4 / 2.0));
    }
}
